/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Christian Connette
 *********************************************************************/

#include <eband_local_planner/eband_visualization.h>

namespace eband_local_planner
{

EBandVisualization::EBandVisualization() : initialized_(false) {}

EBandVisualization::EBandVisualization(ros::NodeHandle &pn, costmap_2d::Costmap2DROS *costmap_ros)
{
    initialize(pn, costmap_ros);
}

EBandVisualization::~EBandVisualization() {}

void EBandVisualization::initialize(ros::NodeHandle &pn, costmap_2d::Costmap2DROS *costmap_ros)
{
    // check if visualization already initialized
    if (!initialized_)
    {
        // read parameters from parameter server
        pn.param("marker_lifetime", marker_lifetime_, 0.5);
        // advertise topics
        one_bubble_pub_ = pn.advertise<visualization_msgs::Marker>("eband_visualization", 1);
        // although we want to publish MarkerArrays we have to advertise Marker topic first -> rviz searchs relative to this
        bubble_pub_ = pn.advertise<visualization_msgs::MarkerArray>("eband_visualization_array", 1);
        // copy adress of costmap and Transform Listener (handed over from move_base) -> to get robot shape
        costmap_ros_ = costmap_ros;
        initialized_ = true;
    }
    else ROS_WARN("Trying to initialize already initialized visualization, doing nothing.");
}

void EBandVisualization::reconfigure(eband_local_planner::EBandPlannerConfig &config)
{
    marker_lifetime_ = config.marker_lifetime;
}

void EBandVisualization::publishBand(std::string marker_name_space, std::vector<Bubble> band)
{
    // check if visualization was initialized
    if (!initialized_)
    {
        ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
        return;
    }
    visualization_msgs::MarkerArray eband_msg;
    eband_msg.markers.resize(band.size());
    visualization_msgs::MarkerArray eband_heading_msg;
    eband_heading_msg.markers.resize(band.size());
    std::string marker_heading_name_space = marker_name_space;
    marker_heading_name_space.append("_heading");
    // convert elastic band to msg
    for (int i = 0; i < ((int) band.size()); i++)
    {
        // convert bubbles in eband to marker msg
        bubbleToMarker(band[i], eband_msg.markers[i], marker_name_space, i, green);
        // convert bubbles in eband to marker msg
        // bubbleHeadingToMarker(band[i], eband_heading_msg.markers[i], marker_heading_name_space, i, green);
    }
    // publish
    bubble_pub_.publish(eband_msg);
    //bubble_pub_.publish(eband_heading_msg);
}

void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Bubble bubble)
{
    // check if visualization was initialized
    if (!initialized_)
    {
        ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
        return;
    }
    visualization_msgs::Marker bubble_msg;
    // convert bubble to marker msg
    bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, green);
    // publish
    one_bubble_pub_.publish(bubble_msg);
}

void EBandVisualization::publishBubble(std::string marker_name_space, int marker_id, Color marker_color, Bubble bubble)
{
    // check if visualization was initialized
    if (!initialized_)
    {
        ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
        return;
    }
    visualization_msgs::Marker bubble_msg;
    // convert bubble to marker msg
    bubbleToMarker(bubble, bubble_msg, marker_name_space, marker_id, marker_color);
    // publish
    one_bubble_pub_.publish(bubble_msg);
}

void EBandVisualization::publishForceList(std::string marker_name_space,
                                          std::vector<geometry_msgs::WrenchStamped> forces,
                                          std::vector<Bubble> band)
{
    // check if visualization was initialized
    if (!initialized_)
    {
        ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
        return;
    }
    visualization_msgs::MarkerArray forces_msg;
    forces_msg.markers.resize(forces.size());
    //before converting to msg - check whether internal, external or resulting forces - switch color
    Color marker_color = green;
    if (marker_name_space.compare("internal_forces") == 0) marker_color = blue;
    if (marker_name_space.compare("external_forces") == 0) marker_color = red;
    if (marker_name_space.compare("resulting_forces") == 0) marker_color = green;
    for (int i = 0; i < ((int) forces.size()); i++)
    {
        // convert wrenches in force list into marker msg
        forceToMarker(forces[i], band[i].center.pose, forces_msg.markers[i], marker_name_space, i, marker_color);
    }
    // publish
    bubble_pub_.publish(forces_msg);
}

void EBandVisualization::publishForce(std::string marker_name_space, int id, Color marker_color,
                                      geometry_msgs::WrenchStamped force, Bubble bubble)
{
    // check if visualization was initialized
    if (!initialized_)
    {
        ROS_ERROR("Visualization not yet initialized, please call initialize() before using visualization");
        return;
    }
    visualization_msgs::Marker force_msg;
    // convert wrenches in force list into marker msg
    forceToMarker(force, bubble.center.pose, force_msg, marker_name_space, id, marker_color);
    // publish
    one_bubble_pub_.publish(force_msg);
}

void EBandVisualization::bubbleToMarker(Bubble bubble, visualization_msgs::Marker &marker, std::string marker_name_space,
                                   int marker_id, Color marker_color)
{
    geometry_msgs::Pose2D tmp_pose2d;
    // header
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = bubble.center.header.frame_id;
    // identifier and cmds
    marker.ns = marker_name_space;
    marker.id = marker_id;
    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;
    // body
    marker.pose = bubble.center.pose;
    // get theta-angle to display as elevation
    PoseToPose2D(bubble.center.pose, tmp_pose2d);
    marker.pose.position.z = 0;//tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_);
    // scale ~ diameter --> is 2x expansion ~ radius
    marker.scale.x = 2.0 * bubble.expansion;
    marker.scale.y = 2.0 * bubble.expansion;
    marker.scale.z = 2.0 * bubble.expansion;
    // color (rgb)
    marker.color.r = 0.0f;
    marker.color.g = 0.0f;
    marker.color.b = 0.0f;
    switch (marker_color)
    {
        case red:
        {
            marker.color.r = 1.0f;
            break;
        }
        case green:
        {
            marker.color.g = 1.0f;
            break;
        }
        case blue:
        {
            marker.color.b = 1.0f;
            break;
        }
    }
    // transparency (alpha value < 1 : displays marker transparent)
    marker.color.a = 0.75;
    // lifetime of this marker
    marker.lifetime = ros::Duration(marker_lifetime_);
}

void EBandVisualization::bubbleHeadingToMarker(Bubble bubble, visualization_msgs::Marker &marker,
                                               std::string marker_name_space, int marker_id, Color marker_color)
{
    geometry_msgs::Pose2D tmp_pose2d;
    // header
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = bubble.center.header.frame_id;
    // identifier and cmds
    marker.ns = marker_name_space;
    marker.id = marker_id;
    marker.type = visualization_msgs::Marker::ARROW;
    marker.action = visualization_msgs::Marker::ADD;
    // body
    marker.pose = bubble.center.pose;
    // get theta-angle to display as elevation
    PoseToPose2D(bubble.center.pose, tmp_pose2d);
    marker.pose.position.z = tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_);
    // scale ~ diameter --> is 2x expansion ~ radius
    marker.scale.x = 0.9;
    marker.scale.y = 0.45;
    marker.scale.z = 0.45;
    // color (rgb)
    marker.color.r = 0.0f;
    marker.color.g = 0.0f;
    marker.color.b = 0.0f;
    switch (marker_color)
    {
        case red:
        {
            marker.color.r = 1.0f;
            break;
        }
        case green:
        {
            marker.color.g = 1.0f;
            break;
        }
        case blue:
        {
            marker.color.b = 1.0f;
            break;
        }
    }
    // transparency (alpha value < 1 : displays marker transparent)
    marker.color.a = 1.0;
    // lifetime of this marker
    marker.lifetime = ros::Duration(marker_lifetime_);
}

void EBandVisualization::forceToMarker(geometry_msgs::WrenchStamped wrench, geometry_msgs::Pose wrench_origin,
                                       visualization_msgs::Marker &marker, std::string marker_name_space,
                                       int marker_id, Color marker_color)
{
    geometry_msgs::Pose2D tmp_pose2d;
    // header
    marker.header.stamp = ros::Time::now();
    marker.header.frame_id = wrench.header.frame_id;
    // identifier and cmds
    marker.ns = marker_name_space;
    marker.id = marker_id;
    marker.type = visualization_msgs::Marker::ARROW;
    marker.action = visualization_msgs::Marker::ADD;
    // body - origin
    marker.pose.position = wrench_origin.position;
    // get theta-angle to display as elevation
    PoseToPose2D(wrench_origin, tmp_pose2d);
    marker.pose.position.z = tmp_pose2d.theta * getCircumscribedRadius(*costmap_ros_);
    // body - orientation of vector (calc. quaternion that transform xAxis into force-vector)
    // check if force different from zero
    if ((wrench.wrench.force.x != 0) || (wrench.wrench.force.y != 0) || (wrench.wrench.torque.z != 0))
    {
        // find AxisAngle Representation then convert to Quaternion
        // (Axis = normalize(vec1) cross normalize(vec2) // cos(Angle) = normalize(vec1) dot normalize(vec2))
        Eigen::Vector3d x_axis(1.0, 0.0, 0.0);
        Eigen::Vector3d target_vec(wrench.wrench.force.x, wrench.wrench.force.y,
                                   wrench.wrench.torque.z / getCircumscribedRadius(*costmap_ros_));
        Eigen::Vector3d rotation_axis(1.0, 0.0, 0.0);
        double rotation_angle = 0.0;
        // get Axis orthogonal to both vectors ()
        x_axis.normalize(); // unneccessary but just in case
        target_vec.normalize();
        if (!(x_axis == target_vec))
        {
            // vector not identical - cross-product defined
            rotation_axis = x_axis.cross(target_vec);
            rotation_angle = x_axis.dot(target_vec);
            rotation_angle = acos(rotation_angle);
        }
        // create AngleAxis representation
        rotation_axis.normalize(); // normalize vector -> otherwise AngleAxis will be invalid!
        const double s = sin(rotation_angle / 2);
        const double c = cos(rotation_angle / 2);
        Eigen::Quaterniond rotate_quat(c, s * rotation_axis.x(), s * rotation_axis.y(), s * rotation_axis.z());
        // transform quaternion back from Eigen to ROS
        // tf::Quaternion orientation_tf;
        geometry_msgs::Quaternion orientation_msg;
        // tf::quaternionEigenToTF(rotate_quat, orientation_tf);
        // tf::quaternionTFToMsg(orientation_tf, orientation_msg);
        tf2::convert(rotate_quat, orientation_msg);
        // finally set orientation of marker
        marker.pose.orientation = orientation_msg;
        // scale ~ diameter --> is 2x expansion ~ radius
        double scale = sqrt(
                (wrench.wrench.force.x * wrench.wrench.force.x) + (wrench.wrench.force.y * wrench.wrench.force.y) +
                ((wrench.wrench.torque.z / getCircumscribedRadius(*costmap_ros_)) *
                 (wrench.wrench.torque.z / getCircumscribedRadius(*costmap_ros_))));
        marker.scale.x = scale; //1.0;
        marker.scale.y = scale; //1.0;
        marker.scale.z = scale; //1.0;
        // color (rgb)
        marker.color.r = 0.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;
        switch (marker_color)
        {
            case red:
            {
                marker.color.r = 1.0f;
                break;
            }
            case green:
            {
                marker.color.g = 1.0f;
                break;
            }
            case blue:
            {
                marker.color.b = 1.0f;
                break;
            }
        }
        // transparency (alpha value < 1 : displays marker transparent)
        marker.color.a = 1.25;
    }
    else
    {
        // force on this bubble is zero -> make marker invisible
        marker.pose.orientation = wrench_origin.orientation; // just take orientation of bubble as dummy-value
        // scale
        marker.scale.x = 0.0;
        marker.scale.y = 0.0;
        marker.scale.z = 0.0;
        // color (rgb)
        marker.color.r = 0.0f;
        marker.color.g = 0.0f;
        marker.color.b = 0.0f;
        // transparency (alpha value < 1 : displays marker transparent)
        marker.color.a = 0.0;
    }
    // lifetime of this marker
    marker.lifetime = ros::Duration(marker_lifetime_);
}

};
